#ifndef _KALMAN_FILTER__KALMAN_H_
#define _KALMAN_FILTER__KALMAN_H_

#include <stdint.h>
#include <arm_math.h>

#define KF_MAX_STATE_DIM	10
#define KF_MAX_MEASURE_DIM	10

struct kalman_filter {
	uint8_t state_dim;
	uint8_t measure_dim;
	float *x;	/* state vector */
	float *P;	/* estimation error covariance */
	float *A;	/* transition matrix */
	float *H;
	float *Q;
	float *R;

	float *u;	/* input vector */
	float *z;	/* measurement vector */
	float dt;	/* time interval */

	void (*process)(void *filter);	/* the parameter filter will convert to a kalman_filter* */
	void (*measurement)(void *filter, float *estimate_output);

	/* P = APA'+Q */
	/* K = PH'/(HPH' + R) */
	void (*computeA)(void *filter);
	void (*computeQ)(void *filter);
	void (*computeH)(void *filter);
	void (*computeR)(void *filter);
};

void kf_predict(struct kalman_filter *filter, float *input, float dt);
void kf_update(struct kalman_filter *filter, float *measure);

#endif
